/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : ekf_uwb_ins.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Nov 25, 2020
  * Co-Author          : Jiapeng Chen
  * Creation Date      : May 10, 2023
  ******************************************************************************
  */
/* USER CODE END Header */

#include "ekf_uwb_ins/ekf_uwb_ins.hpp"

void EKFUWB::EkfUwb_Init()
{
	Update = false;
	dt = 0.005f;

	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取UWB数据
	xQueuePeek(queueFlow,&flow, 0);
	xQueuePeek(queueHeight, &height, 0);

	//参数初始化
	F <<    1.0f,0.0f,0.0f,  dt,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,1.0f,0.0f,0.0f,  dt,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,1.0f,0.0f,0.0f,  dt,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,1.0f,0.0f,0.0f, -dt,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f, -dt,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f, -dt,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,1.0f;

	B <<    0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,
			dt  ,0.0f,0.0f,
			0.0f,dt  ,0.0f,
			0.0f,0.0f,dt  ,
			0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f;

	G <<    0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			dt  ,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,dt  ,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,dt  ,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,dt  ,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,dt  ,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,dt  ;

	H <<    1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f;

	Q <<    0.1f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.1f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.1f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.1f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.1f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.1f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.001f;

	R <<    1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,1.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,2.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.001f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.001f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,0.001f;

	source_temp << 0.0f,0.0f,0.0f;
	target_temp << 0.0f,0.0f,0.0f;
	lamda = 0;
	lamda_Ttemp = 0;
	lamda_temp = 0;
	YawCaliCnt = 0;

	for (int i=0;i<3;i++)
	{
		ekf_uwb_pos_vel.InsPos[i] = 0;
	}

	//初始状态，协方差矩阵
	X_p << uwbPos.uwbPos[0],uwbPos.uwbPos[1],uwbPos.uwbPos[2],flow.flowx,flow.flowy,height.speedZ,0.0f,0.0f,0.0f;

	P_p = P_p = 1e-4f*MatrixXf::Identity(9, 9);
	accUWB << 0.0f, 0.0f, 0.0f;

	isFirstIns = true;
	isYawCali = false;
}

void EKFUWB::EkfUwb_acc()
{
		xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
		xQueuePeek(queueAhrsEuler, &ahrsEuler, 0);
		xQueuePeek(queueUwbPosVel,&ekf_uwb_pos_vel, 0);
		//当yaw角校准后才会进入该函数，在初次进入定点模式时会记录锁尾的角度（飞机相对于地磁北的角度），减去即可得到飞机机头相对于uwb x轴的角度
		yaw = ahrsEuler.Ang[2] - ekf_uwb_pos_vel.yaw_mag_record;

		Rz << cosf(yaw),-sinf(yaw),0.0f,
			  sinf(yaw), cosf(yaw),0.0f,
				0.0f,	  0.0f,	   1.0f;

		a_k << acc.acc[0],acc.acc[1],acc.acc[2];			//机体坐标系下的加速度测量值

		//当前旋转矩阵
		q.w() = ahrsEuler.q[0];
		q.x() = ahrsEuler.q[1];
		q.y() = ahrsEuler.q[2];
		q.z() = ahrsEuler.q[3];
		q.normalize();
		Cb2n = q.toRotationMatrix();

		accUWB += Rz * Cb2n * a_k;
}

void EKFUWB::EkfUwb_Update()
{
	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取UWB数据
	xQueuePeek(queueFlow, &flow, 0);
	xQueuePeek(queueHeight, &height, 0);

	accUWB /= 5;
	u_k << accUWB[0],accUWB[1],accUWB[2]+9.788f;
	accUWB.setZero();
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;

	y << uwbPos.uwbPos[0],uwbPos.uwbPos[1],uwbPos.uwbPos[2],flow.flowx,flow.flowy,height.speedZ;

	//KF迭代过程

	//Predict state
	X_p = F * X_k + B * u_k;

	//Predict covariance matrix
	//	P_p = F * P_k * F.transpose() + G * Q * G.transpose();
	P_p = F * P_k * F.transpose() + Q;

	//Computer Kalman Gain
	K = P_p * H.transpose() * (H * P_p * H.transpose() + R).inverse();

	//Update state
	X_k = X_p + K * (y - H * X_p);

	//Update covariance matrix
	P_k = (MatrixXf::Identity(9, 9) - K * H) * P_p;


	ekf_uwb_pos_vel.Pos[0] = X_k[0];
	ekf_uwb_pos_vel.Pos[1] = X_k[1];
	ekf_uwb_pos_vel.Pos[2] = X_k[2];

	ekf_uwb_pos_vel.Vel[0] = X_k[3];
	ekf_uwb_pos_vel.Vel[1] = X_k[4];
	ekf_uwb_pos_vel.Vel[2] = X_k[5];

	xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);

	getTimer_us(&stopTimer);
	executionTime_us = stopTimer - startTimer;
}

void EKFUWB::EkfUwb_Ins()
{
		startTimerLast1 = startTimer1;
		getTimer_us(&startTimer1);
		cycleTime_us1 = startTimer1 - startTimerLast1;

		xQueuePeek(queueUWB, &uwbPos, 0);
		xQueuePeek(queueFlow, &flow, 0);
		xQueuePeek(queueHeight, &height, 0);

		if(isFirstIns)
		{
			//记录当前位置，之后的InsPos均为基于当前点的相对位置
			for(int i=0;i<3;i++) ekf_uwb_pos_vel.InsPos[i] = uwbPos.uwbPos[i] + 0.001;
			for(int i=0;i<3;i++) ekf_uwb_pos_vel.UwbPos[i] = uwbPos.uwbPos[i] + 0.001;
			xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);
			osSemaphoreRelease(semTran);
			isFirstIns = false;
		}
		else
		{
			ekf_uwb_pos_vel.InsPos[0] += flow.flowx * dt;
			ekf_uwb_pos_vel.InsPos[1] += flow.flowy * dt;
			//z轴坐标与uwb对齐，+0.0001f防止值为0后续求取出现奇异值
			ekf_uwb_pos_vel.InsPos[2] = uwbPos.uwbPos[2] + 0.001;
			for(int i=0;i<3;i++) ekf_uwb_pos_vel.UwbPos[i] = uwbPos.uwbPos[i] + 0.001;
		}
		getTimer_us(&stopTimer1);
		executionTime_us1 = stopTimer1 - startTimer1;
}

void EKFUWB::EkfUwb_YawCali()
{
	getTimer_us(&startTimer2);

	if(YawCaliCnt > yaw_cali_window)
	{
		isYawCali = true;
		YawCaliCnt = yaw_cali_window + 1;
	}
	else if(YawCaliCnt == yaw_cali_window)
	{
		//求重心向量
		for(int i=0;i<3;i++)
		{
			target_temp(i) /= YawCaliCnt / yaw_cali_window_depart;
			source_temp(i) /= YawCaliCnt / yaw_cali_window_depart;
		}
		//求去除重心向量后的位置信息
		for(int j=0;j<YawCaliCnt / yaw_cali_window_depart;j++)
		{
			for(int i=0;i<3;i++)
			{
				target_xyz(i,j) -= target_temp(i);
				source_xyz(i,j) -= source_temp(i);
			}
		}
		//求lamda
		for(int i=0;i<3;i++)
		{
			for(int j=0;j<YawCaliCnt / yaw_cali_window_depart;j++)
			{
				lamda_Ttemp += pow(target_xyz(i,j),2);
				lamda_temp += pow(source_xyz(i,j),2);
			}
		}
		lamda = sqrt(lamda_Ttemp / lamda_temp);
		//求奇异值分解及旋转矩阵
		A = lamda * source_xyz * target_xyz.transpose();

		JacobiSVD<Eigen::MatrixXf> svd(A, ComputeThinU | ComputeThinV);
		V = svd.matrixV();U = svd.matrixU();
		Rotation = V * U.transpose();
		//求偏航角
		yaw_cali = atanf(Rotation(1,0) / Rotation(0,0));
		//限制角度为0-180
		if(yaw_cali < 0)
		{
			yaw_cali += 360.f / 180.f * M_PI;
		}
		ekf_uwb_pos_vel.yaw_cali = yaw_cali;
		xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);
	}
	else
	{
		if(YawCaliCnt % yaw_cali_window_depart == 0)
		{
			for(int i=0;i<3;i++)
			{
				//+0.0001f防止后续求取出现奇异值
				target_xyz(i,YawCaliCnt / yaw_cali_window_depart) = ekf_uwb_pos_vel.UwbPos[i] + 0.001;
				source_xyz(i,YawCaliCnt / yaw_cali_window_depart) = ekf_uwb_pos_vel.InsPos[i];
				//求重心
				target_temp(i) += target_xyz(i,YawCaliCnt / yaw_cali_window_depart);
				source_temp(i) += source_xyz(i,YawCaliCnt / yaw_cali_window_depart);
			}
			xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);
		}
	}

	YawCaliCnt ++;

	getTimer_us(&stopTimer2);
	executionTime_us2 = stopTimer2 - startTimer2;
}

bool EKFUWB::EkfUwb_isYawCali()
{
	return isYawCali;
}

EKFUWB ekfUwb((char *)"EKF_UWB_INS");

extern "C" void ekfUwb1ms_main(void *argument)
{
	osDelay(2000);
	ekfUwb.EkfUwb_Init();
	osDelay(200);
	for(;;)
	{
		osSemaphoreAcquire(semEkfUwb1ms,0xffffffff);
		if(ekfUwb.EkfUwb_isYawCali()) ekfUwb.EkfUwb_acc();
	}
}

extern "C" void ekfUwb5ms_main(void *argument)
{
	for(;;)
	{
		osSemaphoreAcquire(semEkfUwb5ms,0xffffffff);
		if(ekfUwb.EkfUwb_isYawCali())
		{
			ekfUwb.EkfUwb_Update();
		}
		else
		{
			if(osSemaphoreAcquire(semEkfIns,0x00) == osOK)
			{
				ekfUwb.EkfUwb_Ins();
				ekfUwb.EkfUwb_YawCali();
			}
		}
	}
}
/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
